#include <pob-eye.h>
#define DEBUG

/**
 *
 *Fuzzy-Pob
 *@version 0.1
 *@author spainvictorcompany
 *
 */

/**
 * Distance sensor parameter :
 *
 * Number after the symbol name are the PORTA's pin of your distance sensor.
 *
 * DISTANCE_SENSOR_RIGHT : sensor at the right of the pob-bot
 * DISTANCE_SENSOR_LEFT : sensor at the left of the pob-bot
 *
 */
#define DISTANCE_SENSOR_RIGHT 5
#define DISTANCE_SENSOR_LEFT 3

//function for pob-proto and move bot
static void InitPobProto (void);
static void MoveAndStop(UInt8 Way,UInt32 time);
static void MoveBot(UInt8 Way);

//move order for pob-proto board
#define MOVE_RUN		0x05
#define MOVE_BACK		0x0A
#define MOVE_LEFT		0x09
#define MOVE_RIGHT		0x06
#define STOP_BOT		0

/**
 *function for get CD value
 *@param void
 *@return CD is a sensor right value
 */
int getCD(void)
{
	int CD = GetPortAnalog(DISTANCE_SENSOR_RIGHT);
#ifdef DEBUG
	PrintTextOnPobTerminal("CD = %d\n" , CD);
#endif
	return CD;
}
/**
 *function for get CG value
 *@param void
 *@return CG is a sensor left value
 */
int getCG(void)
{
	int CG = GetPortAnalog(DISTANCE_SENSOR_LEFT);
#ifdef DEBUG
	PrintTextOnPobTerminal("CG = %d\n" , CG);
#endif
	return CG;
}
/**
 *function for calculate DApres of the right sensor
 *@param CD
 *@return DApresCD
 */
int DApresCD(CD)
{
    if (CD <= 128 && 102 <= CD)
	{
		int DApresCD = 100;
#ifdef DEBUG
	PrintTextOnPobTerminal("DApresCD = %d\n" , DApresCD);
#endif
		return DApresCD ;
	}
	else if (CD > 30)
	{
		int DApresCD = (int)(((CD - 30) * 100) / 72) ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DApresCD = %d\n" , DApresCD);
#endif
		return DApresCD;
	}
	else
	{
		int DApresCD = 0;
#ifdef DEBUG
	PrintTextOnPobTerminal("DApresCD = %d\n" , DApresCD);
#endif
		return DApresCD;
	}
}
/**
 *function for calculate DApres of the left sensor
 *@param CG
 *@return DApresCG
 */
int DApresCG(CG)
{
    if (CG <= 128 && 102 <= CG)
	{
		int DApresCG = 100 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DApresCG = %d\n" , DApresCG);
#endif
		return DApresCG;
    }
	else if (CG > 30)
	{
        int DApresCG = (int)(((CG - 30) * 100) / 72) ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DApresCG = %d\n" , DApresCG);
#endif
        return DApresCG;
    }
    else
    {
        int DApresCG = 0 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DApresCG = %d\n" , DApresCG);
#endif
        return DApresCG;
    }
}
/**
 *function for calculate DAloin of the right sensor
 *@param CD
 *@return DAloinCD
 */
int DAloinCD(CD)
{
    if (CD <= 30 && CD >= 1)
	{
		int DAloinCD = 100 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DAloinCD = %d\n" , DAloinCD);
#endif
		return DAloinCD;
    }
	else if (CD < 63)
	{
        int DAloinCD = (int)(((CD - 63) * 100) / -33);
#ifdef DEBUG
	PrintTextOnPobTerminal("DAloinCD = %d\n" , DAloinCD);
#endif
        return DAloinCD;
    }
    else
    {
        int DAloinCD = 0 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DAloinCD = %d\n" , DAloinCD);
#endif
        return DAloinCD;
    }
}
/**
 *function for calculate DAloin of the left sensor
 *@param CG
 *@return DAloinCG
 */
int DAloinCG(CG)
{
	if (CG <= 30 && CG >= 1)
	{
		int DAloinCG = 100 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DAloinCG = %d\n" , DAloinCG);
#endif
		return DAloinCG;
    }
	else if (CG < 63)
	{
        int DAloinCG = (int)(((CG - 63) * 100) / -33);
#ifdef DEBUG
	PrintTextOnPobTerminal("DAloinCG = %d\n" , DAloinCG);
#endif
        return DAloinCG;
    }
    else
    {
        int DAloinCG = 0;
#ifdef DEBUG
	PrintTextOnPobTerminal("DAloinCG = %d\n" , DAloinCG);
#endif
        return DAloinCG;
    }
}
/**
 *function for calculate DAbeaucoupDG
 *@param DApresCD
 *@param DApresCG
 *@return DAbeaucoupDG
 */
int regle1(DApresCD , DApresCG)
{
	if ((DApresCD != 0) && (DApresCG != 0) && (DApresCD >= DApresCG))
	{
        int DAbeaucoupDG = DApresCD ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DAbeaucoupDG = %d\n" , DAbeaucoupDG);
#endif
        return DAbeaucoupDG ;
	}
	else
        return 1;
}
/**
 *function for calculate DAbeaucoupDD
 *@param DApresCD
 *@param DApresCG
 *@return DAbeaucoupDD
 */
int regle2(DApresCD , DApresCG)
{
    if ((DApresCD != 0) && (DApresCG != 0) && (DApresCD <= DApresCG))
	{
		int DAbeaucoupDD = DApresCG;
#ifdef DEBUG
	PrintTextOnPobTerminal("DAbeaucoupDD = %d\n" , DAbeaucoupDD);
#endif
		return DAbeaucoupDD ;
	}
	else
        return 1;
}
/**
 *function for calculate DApeuDG
 *@param DAloinCD
 *@param DAloinCG
 *@return DApeuDG
 */
int regle3(DAloinCD , DAloinCG)
{
	if ((DAloinCD != 0) && (DAloinCG != 0) && (DAloinCD >= DAloinCG ))
	{
		int DApeuDG = DAloinCD;
#ifdef DEBUG
	PrintTextOnPobTerminal("DApeuDG = %d\n" , DApeuDG);
#endif
		return DApeuDG ;
	}
	else
        return 1;
}
/**
 *function for calculate DApeuDD
 *@param DAloinCD
 *@param DAloinCG
 *@return DApeuDD
 */
int regle4(DAloinCD , DAloinCG)
{
    if ((DAloinCD != 0) && (DAloinCG != 0) && (DAloinCD <= DAloinCG))
	{
		int DApeuDD = DAloinCG;
#ifdef DEBUG
	PrintTextOnPobTerminal("DApeuDD = %d\n" , DApeuDD);
#endif
		return DApeuDD ;
	}
	else
        return 1;
}
/**
 *function for calculate DDpeu
 *@param DApeuDD
 *@return DDpeu
 */
int DDPeu(DApeuDD)
{
    if (DApeuDD > 0)
	{
		int DDpeu = (int)((-30 * DApeuDD) / 100) + 50 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DDpeu = %d\n" , DDpeu);
#endif
		return DDpeu ;
	}
	else
	{
        int DDpeu = 0;
#ifdef DEBUG
	PrintTextOnPobTerminal("DDpeu = %d\n" , DDpeu);
#endif
        return DDpeu;
	}
}
/**
 *function for calculate DDbeaucoup
 *@param DAbeaucoupDD
 *@return DDbeaucoup
 */
int DDBeaucoup(DAbeaucoupDD)
{
    if (DAbeaucoupDD > 0)
	{
		int DDbeaucoup = (int)(( 30 * DAbeaucoupDD) / 100 ) + 40 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DDbeaucoup = %d\n" , DDbeaucoup);
#endif
		return DDbeaucoup ;
	}
	else
	{
        int DDbeaucoup = 0;
#ifdef DEBUG
	PrintTextOnPobTerminal("DDbeaucoup = %d\n" , DDbeaucoup);
#endif
        return DDbeaucoup;
	}
}
/**
 *function for calculate DGpeu
 *@param DApeuDG
 *@return DGpeu
 */
int DGPeu(DApeuDG)
{

    if (DApeuDG > 0)
	{
		int DGpeu = (int)(( -30 * DApeuDG) / 100 ) + 50 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DGpeu = %d\n" , DGpeu);
#endif
		return DGpeu;

	}
	else
	{
	    int DGpeu = 0;
#ifdef DEBUG
	PrintTextOnPobTerminal("DGpeu = %d\n" , DGpeu);
#endif
        return DGpeu;
	}
}
/**
 *function for calculate DGbeaucoup
 *@param DAbeaucoupDG
 *@return DGbeaucoup
 */
int DGBeaucoup(DAbeaucoupDG)
{
    if (DAbeaucoupDG > 0)
	{
		int DGbeaucoup = (int)(( 30 * DAbeaucoupDG) / 100 ) + 40 ;
#ifdef DEBUG
	PrintTextOnPobTerminal("DGbeaucoup = %d\n" , DGbeaucoup);
#endif
		return DGbeaucoup;

	}
	else
	{
        int DGbeaucoup = 0;
#ifdef DEBUG
	PrintTextOnPobTerminal("DGbeaucoup = %d\n" , DGbeaucoup);
#endif
        return DGbeaucoup;
	}

}
/**
 *function to convert DDpeu in a real action
 *@param DDpeu
 *@return 0 action
 *@return 1 no action
 */
int convertirDDpeu(DDpeu)
{
	if (DDpeu != 0)
	{
        MoveBot(MOVE_RIGHT);
        int temp = (int)(( DDpeu * 2000000 ) / 90) ;
        Wait( temp );
        return 0;
	}
	else
        return 1;

}
/**
 *function to convert DDbeaucoup in a real action
 *@param DDbeaucoup
 *@return 0 action
 *@return 1 no action
 */
int convertirDDbeaucoup(DDbeaucoup)
{
	if (DDbeaucoup != 0)
	{
        MoveBot(MOVE_RIGHT);
        int temp =(int)((DDbeaucoup * 2000000 ) / 90) ;
        Wait( temp );
        return 0;
	}
	else
        return 1;

}
/**
 *function to convert DGpeu in a real action
 *@param DGpeu
 *@return 0 action
 *@return 1 no action
 */
int convertirDGpeu(DGpeu)
{
	if (DGpeu != 0)
	{
        MoveBot(MOVE_LEFT);
        int temp =(int)((DGpeu * 2000000 ) / 90) ;
        Wait( temp );
        return 0;
	}
	else
        return 1;

}
/**
 *function to convert DGbeaucoup in a real action
 *@param DGbeaucoup
 *@return 0 action
 *@return 1 no action
 */
int convertirDGbeaucoup(DGbeaucoup)
{
	if (DGbeaucoup != 0)
	{
        MoveBot(MOVE_LEFT);
        int temp =(int)((DGbeaucoup * 2000000 ) / 90) ;
        Wait( temp );
        return 0;
	}
	else
        return 1;

}

/**
 *main function
 *@param void
 *@return 0 exit ok
 */
int main( void )
{
	InitPOBEYE();
	InitPobProto();
	PobProto Proto ;

	while(1)
	{

	    if(getCD() < 20 && getCG() < 20)
	    {
	        MoveBot(MOVE_RUN);
            Wait(2000000);
	    }
        else
        {
            convertirDDpeu(DDPeu(regle4(DAloinCD(getCD()) , DAloinCG(getCG()))));
            convertirDDbeaucoup(DDBeaucoup(regle2(DApresCD(getCD()) , DApresCG(getCG()))));
            convertirDGpeu(DGPeu(regle3(DAloinCD(getCD()) , DAloinCG(getCG()))));
            convertirDGbeaucoup(DGBeaucoup(regle1(DApresCD(getCD()) , DApresCG(getCG()))));
	    }
    }

	return 0;
}

/**
 * Initialize the POB-PROTO board. PORTA set as analogic, PORTC as servomotor.
 *
 */
void InitPobProto (void)
{
	PobProto board;
	//to get the position of the analogic joystick, you have to set the PORTA as analogic input
	board.porta=ALL_PORTA_AS_ANA;
    //all pin of PORTC are configured to manage servomotors
	board.portc=RC7_AS_SERVO |RC6_AS_SERVO |RC3_AS_SERVO |RC2_AS_SERVO |RC1_AS_SERVO |RC0_AS_SERVO;
	//RD0 RD1 RD2 RD3 are configured as digitals output to gear DC motor
	//RD4 RD5 RD6 RD7 are configured as digitals input
	board.portd=RD7_AS_DI |RD6_AS_DI |RD5_AS_DI |RD4_AS_DI |RD3_AS_DO |RD2_AS_DO |RD1_AS_DO |RD0_AS_DO;
	//set the pob proto
	SetPobProto(&board);

}

/**
 * Move and stop bot.
 *
 *@param Way : move order
 *@param time : time to move
 *
 */
void MoveAndStop( UInt8 Way, UInt32 time )
{
	MoveBot(Way);
	Wait(time);
	MoveBot(STOP_BOT);
}

/**
 * Move the bot
 *
 * @param Way : move bot order
 *
 */
void MoveBot (UInt8 Way)
{
	SetPortD(Way);

}
